#include "speed_cntr.h"
#include "device.h"
#include "modbus.h"

//! Cointains data for timer interrupt.
SpeedRampData srd_y = {0};
SpeedRampData srd_z = {0};
#if (PCB_VERSION == PCB_A2)
SpeedRampData srd_m = {0};
#endif

#if MOVE_METHOD == MOVE_METHOD_INTERP
struct SpeedInterp
{
    long xstep;
    long ystep;
    long err;
    float speedFactor;
    float revSpeedFactor;
    SpeedRampData *master;
} si;
#endif
#ifdef AVOID_OBSTACLES
ushort speed_cntr_max_index(ushort speed)
{
    ushort max_index = (ulong)speed * speed * ( 1 / (2 * ALPHA * ACCEL * 100));
    if (max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    return max_index;
}
#endif
void speed_cntr_Move(SpeedRampData *srd, long step, unsigned int speed)
{
    if (step == 0)
        return;

    // Set direction from sign on step value.
    if(step < 0)
    {
        srd->dir = CCW;
        srd->step_offset = -1;
        step = -step;
    }
    else
    {
        srd->dir = CW;
        srd->step_offset = 1;
    }

    // 最大速度对应的速度表索引
    // n = speed^2 / (2*alpha*accel)
    srd->max_index = (ulong)speed * speed * ( 1 / (2 * ALPHA * ACCEL * 100));
    if (srd->max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    srd->max_ideal_index = srd->max_index;
    srd->max_ref_index = srd->max_index;

    srd->speed_index = 0;
    srd->step_left = step;
    srd->run_state = MOTION_STATE_RUN;

    srd->hit_home = 0;
    srd->hit_limit = 0;
    srd->running = 1;
    srd->need_cleanup = 0;
    srd->ref_changed = 0;
    srd->start_time = SystemTick();
    srd->step_count = 0;
    srd->step_correction = 0;
    srd->init();
}

void speed_cntr_update(SpeedRampData *srd) reentrant
{
    if (!srd->running)
        return;

#if MOVE_METHOD != MOVE_METHOD_INTERP
    #ifdef PULSE_DUTY_CYCLES_50
        srd->start_timer(FREQ_TABLE(srd->speed_index) >> 1);
    #else
        srd->start_timer(FREQ_TABLE(srd->speed_index));
    #endif// PULSE_DUTY_CYCLES_50
#endif// MOVE_METHOD

    if (srd->run_state == MOTION_STATE_STOP)
    {
#if MOVE_METHOD != MOVE_METHOD_INTERP
        srd->stop_timer();
#endif
        srd->need_cleanup = 1;
    }
    else
    {
#if !defined(PULSE_DUTY_CYCLES_50) || (MOVE_METHOD == MOVE_METHOD_INTERP)
        // 输出高电平
        srd->step(1);
#else
        srd->output = !srd->output;
        srd->step(srd->output);
        if (srd->output)
            return;
#endif

        // 更新已经运行的步数和剩余步数
        srd->step_count++;
        srd->step_left--;

        // 走一步, 调用更新函数, 用以检查传感器状态, 更新剩余步数等
        srd->update();

        if (srd->step_left <= 0)
        {
            srd->run_state = MOTION_STATE_STOP;
#if defined(PULSE_DUTY_CYCLES_50) && MOVE_METHOD != MOVE_METHOD_INTERP
            srd->stop_timer();
            srd->need_cleanup = 1;
#endif
        }
        else
        {
            // 如果参考坐标系改变, 更新最大输出速度
            if (srd->ref_changed)
            {
                srd->max_index = min(srd->max_ideal_index, srd->max_ref_index);
                srd->ref_changed = 0;
            }

            // 更新下一步速度
            if (srd->speed_index > srd->step_left)
                srd->speed_index -= srd->speed_index / (ushort)srd->step_left;
            else if (srd->speed_index > srd->max_index)
                srd->speed_index--;
            else if (srd->speed_index < srd->max_index)
                srd->speed_index++;
        }

#if !defined(PULSE_DUTY_CYCLES_50) || (MOVE_METHOD == MOVE_METHOD_INTERP)
        // 输出低电平
        srd->step(0);
#endif
    }
}

#if MOVE_METHOD == MOVE_METHOD_INTERP
void speed_cntr_interp(long xstep, long ystep, unsigned int speed)
{
    srd_x.interp = 1;
    srd_y.interp = 1;
    speed_cntr_Move(&srd_x, xstep, speed);
    speed_cntr_Move(&srd_y, ystep, speed);

    si.xstep = srd_x.step_left;
    si.ystep = srd_y.step_left;
    si.err = si.xstep - si.ystep;

    if (si.err >= 0)
    {
        si.master = &srd_x;
        si.speedFactor = 1.0;
    }
    else
    {
        si.master = &srd_y;
        si.speedFactor = (float)si.xstep / si.ystep;
    }
    si.revSpeedFactor = 1.0 / si.speedFactor;

    TF2H = 1;
}

void speed_cntr_interp_update()
{
    long e2 = si.err << 1;
    ushort tick = SystemTickLow() + FREQ_TABLE(si.master->speed_index);

    tick = -tick;
    TR2 = 0;
    TMR2L = tick;
    TMR2H = tick >> 8;
    TR2 = 1;

    if (srd_x.run_state == MOTION_STATE_STOP &&
            srd_y.run_state == MOTION_STATE_STOP)
    {
        speed_cntr_update(&srd_x);
        speed_cntr_update(&srd_y);
        TMR2CN &= ~0x84;
    }
    else
    {
        if (e2 > -si.ystep)
        {
            speed_cntr_update(&srd_x);
            if (srd_x.step_correction)
            {
                si.xstep += srd_x.step_correction;
                srd_x.step_correction = 0;
            }
            si.err -= si.ystep;
        }

        if (e2 < si.xstep)
        {
            speed_cntr_update(&srd_y);
            if (srd_y.step_correction)
            {
                si.ystep += srd_y.step_correction;
                srd_y.step_correction = 0;
            }
            si.err += si.xstep;
        }
    }
}
#endif

#ifdef ENABLE_REF_SPEED
void Update_Ref_Speed()
{
    short speed;
    long max_speed;

    if (srd_x.running)
    {
        EA = 0;
        EA = 0;
#if MOVE_METHOD == MOVE_METHOD_INTERP
        speed = FREQ_TABLE(si.master->speed_index);
#else
        speed = FREQ_TABLE(srd_x.speed_index);
#endif
        EA = 1;

        // speed = alpha / t = alpha * t_freq / delay
#if MOVE_METHOD == MOVE_METHOD_INTERP
        x_speed = ((ulong)((ALPHA * SYSCLK / 12 * 100) * si.speedFactor)) / (ushort)speed;
#else
        x_speed = ((ulong)(ALPHA * SYSCLK / 12 * 100)) / (ushort)speed;
#endif

        if (srd_x.dir == CCW)
            x_speed = -x_speed;

        // 根据参考系的速度计算当前最大速度索引
        if (ref_changed && !srd_x.ref_changed && srd_x.dir == CW)
        {
            // 相距(x_ref_pos - x_pos), 在0.05秒内不相撞的最大速度
#if MOVE_METHOD == MOVE_METHOD_INTERP
            max_speed = (x_ref_pos - x_pos) * (ALPHA * 100 / 0.05) * si.revSpeedFactor + x_ref_speed;
#else
            max_speed = (x_ref_pos - x_pos) * (ALPHA * 100 / 0.05) + x_ref_speed;
#endif

            if (max_speed <= 0)
                srd_x.max_ref_index = 0;
            else
                srd_x.max_ref_index = max_speed * max_speed * ( 1 / (2 * ALPHA * ACCEL * 100));

            srd_x.ref_changed = 1;
        }
    }
    else
        x_speed = 0;
}
#endif
